/*
 * ________________________________________________________________________________________________________
 * Copyright (c) 2016-2016 InvenSense Inc. All rights reserved.
 *
 * This software, related documentation and any modifications thereto (collectively ?Software?) is subject
 * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
 * and other intellectual property rights laws.
 *
 * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
 * and any use, reproduction, disclosure or distribution of the Software without an express license agreement
 * from InvenSense is strictly prohibited.
 *
 * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
 * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
 * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
 * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
 * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
 * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
 * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
 * OF THE SOFTWARE.
 * ________________________________________________________________________________________________________
 */

/* board driver */

#include "icm42688_rt1064.h"
#include "fsl_gpio.h"
#include "peripherals.h"

#include "Invn/EmbUtils/Message.h"
Buffer_t timestamp_buffer;
int irq_from_device;
uint8_t dataTx[SPI_BUFFER_SIZE];
uint8_t dataRx[SPI_BUFFER_SIZE];

/* Buffer to keep track of the timestamp when icm426xx data ready interrupt fires. */
#define SPI_BUFFER_SIZE 8220
/* I2C number and slave address for INV device */
#if (SM_BOARD_REV == SM_REVB_OB)
#define INV_SPI_AP INV_SPI_ONBOARD_REVB
#define ICM_I2C_ADDR 0x69
#else
/* SM_REVB_DB and SM_REVG have same SPI/I2C configuration */
#define INV_SPI_AP INV_SPI_REVG
#define ICM_I2C_ADDR 0x68
#endif

/* This variable contains the number of nested calls to disable_irq */
static uint32_t sDisableIntCount = 0;

int inv_io_hal_configure(struct inv_icm426xx_serif *serif)
{
	switch (serif->serif_type)
	{
	default:
		return -1;
	}
}

int inv_io_hal_read_reg(struct inv_icm426xx_serif *serif, uint8_t reg, uint8_t *rbuffer,
						uint32_t rlen)
{
	if (rlen + 1 > SPI_BUFFER_SIZE)
		return 1;

	lpspi_transfer_t masterXfer;
	dataTx[0] = reg | 0x80;
	memset(&dataTx[1],0x00,rlen);
	switch (serif->serif_type)
	{
	case ICM426XX_AUX1_SPI3:
	case ICM426XX_AUX1_SPI4:
	case ICM426XX_AUX2_SPI3:
	case ICM426XX_UI_SPI4:
	{
		masterXfer.txData = dataTx;
		masterXfer.rxData = dataRx;
		masterXfer.dataSize = rlen + 1;
		masterXfer.configFlags =
			LPSPI_IMU_PCS_FOR_TRANSFER | kLPSPI_MasterPcsContinuous | kLPSPI_MasterByteSwap;
		//GPIO_PinWrite(GPIO2,20,0); 
		 LPSPI_MasterTransferBlocking(LPSPI_IMU_MASTER_BASEADDR, &masterXfer);
		//GPIO_PinWrite(GPIO2,20,1); 
		 memcpy((uint8_t *)rbuffer,&dataRx[1],rlen);
		inv_icm426xx_sleep_us(2);
		
		return 0;
	}
	/*case ICM426XX_UI_I2C:
		while (inv_i2c_master_read_register(ICM_I2C_ADDR, reg, rlen, rbuffer)) {
			inv_delay_us(32000); // Loop in case of I2C timeout
		}
		return 0;*/
	default:
		return -1;
	}
}

int inv_io_hal_write_reg(struct inv_icm426xx_serif *serif, uint8_t reg, const uint8_t *wbuffer,
						 uint32_t wlen)
{
	if (wlen + 1 > SPI_BUFFER_SIZE)
		return 1;
	int rc;
	//uint8_t dataTx[2];
	lpspi_transfer_t masterXfer;
	switch (serif->serif_type)
	{
	case ICM426XX_AUX1_SPI3:
	case ICM426XX_AUX1_SPI4:
	case ICM426XX_AUX2_SPI3:
	case ICM426XX_UI_SPI4:
		for (uint32_t i = 0; i < wlen; i++)
		{
			dataTx[0] = reg + i;
			memcpy(&dataTx[1], &wbuffer[i], 1);
			masterXfer.txData = dataTx;
			masterXfer.rxData = NULL;
			masterXfer.dataSize = 2;
			masterXfer.configFlags =
				LPSPI_IMU_PCS_FOR_TRANSFER | kLPSPI_MasterPcsContinuous | kLPSPI_MasterByteSwap;
			//rc = SpiWriteData(reg + i, 1, &wbuffer[i]);
			//GPIO_PinWrite(GPIO2,20,0); 
			rc = LPSPI_MasterTransferBlocking(LPSPI_IMU_MASTER_BASEADDR, &masterXfer);
			//GPIO_PinWrite(GPIO2,20,1); 
			if (rc)
				return rc;
		}
		return 0;
	/*case ICM426XX_UI_I2C:
		while (inv_i2c_master_write_register(ICM_I2C_ADDR, reg, wlen, wbuffer)) {
			inv_delay_us(32000); // Loop in case of I2C timeout
		}
		return 0;*/
	default:
		return -1;
	}
}

/*
 * Icm426xx driver needs to get time in us. Let's give its implementation here.
 */
//��Ҫ����
uint64_t inv_icm426xx_get_time_us(void)
{
	return getTimeInUs();
}

void inv_icm426xx_sleep_us(uint32_t us)
{
	Delay_us(us);
}

void inv_disable_irq(void)
{
	if (sDisableIntCount == 0)
	{
		__set_BASEPRI(1);
	}
	sDisableIntCount++;
}
void inv_enable_irq(void)
{
	sDisableIntCount--;
	if (sDisableIntCount == 0)
	{
		//__enable_irq();
		__set_BASEPRI(0);
	}
}

void BOARD_INITPINS_IMU0_INT1_callback(void *param)
{
	uint64_t timestamp = inv_icm426xx_get_time_us();
	if (!RINGBUFFER_FULL(&timestamp_buffer))
	{
		RINGBUFFER_PUSH(&timestamp_buffer, &timestamp);
	}
	irq_from_device |= TO_MASK(1);
}

/*
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	uint64_t timestamp = inv_icm426xx_get_time_us();
	if (GPIO_Pin == GPIO_PIN_3 && !RINGBUFFER_FULL(&timestamp_buffer))
	{
		RINGBUFFER_PUSH(&timestamp_buffer, &timestamp);
	}

	irq_from_device |= TO_MASK(GPIO_Pin);
}*/